/*
    MIT License

    Copyright (c) 2021 Zhepei Wang (wangzhepei@live.com)

    Permission is hereby granted, free of charge, to any person obtaining a copy
    of this software and associated documentation files (the "Software"), to deal
    in the Software without restriction, including without limitation the rights
    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
    copies of the Software, and to permit persons to whom the Software is
    furnished to do so, subject to the following conditions:

    The above copyright notice and this permission notice shall be included in all
    copies or substantial portions of the Software.

    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
    SOFTWARE.
*/

#ifndef GCOPTER_HPP
#define GCOPTER_HPP

#include "gcopter/minco.hpp"
#include "gcopter/geo_utils.hpp"
#include "gcopter/lbfgs.hpp"
#include "gcopter/fastcheck_traj.hpp"

#include <Eigen/Eigen>

#include <cmath>
#include <cfloat>
#include <iostream>
#include <vector>

using namespace std;
namespace gcopter
{

    class GCOPTER_PolytopeSFC
    {
    public:
        typedef Eigen::Matrix3Xd PolyhedronV;
        typedef Eigen::MatrixX4d PolyhedronH;
        typedef std::vector<PolyhedronV> PolyhedraV;
        typedef std::vector<PolyhedronH> PolyhedraH;

    private:
        minco::MINCO_S3NU minco;
        // flatness::FlatnessMap flatmap;

        double rho, phi;
        double minT;
        Eigen::Matrix3d headPVA;
        Eigen::Matrix3d tailPVA;

        Eigen::Matrix3Xd vEndVel;
        PolyhedraV vPolytopes;
        PolyhedraH hPolytopes;
        std::vector<FastCheckTraj> swarm_trajs_;
        Eigen::Matrix3Xd shortPath;

        Eigen::VectorXi pieceIdx;
        Eigen::VectorXi vPolyIdx;
        Eigen::VectorXi hPolyIdx;

        int polyN;
        int pieceN;

        int spatialDim;
        int temporalDim;
        int endVelDim;

        double smoothEps;
        int integralRes;
        Eigen::VectorXd magnitudeBd;
        Eigen::VectorXd penaltyWt;
        // Eigen::VectorXd physicalPm;
        double allocSpeed;

        lbfgs::lbfgs_parameter_t lbfgs_params;

        Eigen::Matrix3Xd points; //interpoints
        Eigen::VectorXd times;
        Eigen::Matrix3Xd gradByPoints; //interpoints grad
        Eigen::VectorXd gradByTimes;
        Eigen::VectorXd gradByEndVel;
        Eigen::MatrixX3d partialGradByCoeffs; 
        Eigen::VectorXd partialGradByTimes; 

    private:
        /**
         * @brief get T from tau
         * 
         * @param tau \in R^n
         * @param T   the interval of each segment
         */
        static inline void forwardT(const Eigen::VectorXd &tau,
                                    Eigen::VectorXd &T)
        {
            const int sizeTau = tau.size();
            T.resize(sizeTau);
            for (int i = 0; i < sizeTau; i++)
            {
                T(i) = tau(i) > 0.0
                           ? ((0.5 * tau(i) + 1.0) * tau(i) + 1.0)
                           : 1.0 / ((0.5 * tau(i) - 1.0) * tau(i) + 1.0);
            }
            return;
        }

        /**
         * @brief  map T to tau
         *      since optimizer wishes to optimize tau without constraints, while T >= 0.
         *      tau > 0: T = 1/2 tau^2 + tau + 1; tau <= 0: T = 2/(tau^2 - tau + 1); R ---> R+
         * @tparam EIGENVEC 
         * @param T   the interval of each segment
         * @param tau \in R^n
         */
        template <typename EIGENVEC>
        static inline void backwardT(const Eigen::VectorXd &T,
                                     EIGENVEC &tau)
        {
            const int sizeT = T.size();
            tau.resize(sizeT);
            for (int i = 0; i < sizeT; i++)
            {
                tau(i) = T(i) > 1.0
                             ? (sqrt(2.0 * T(i) - 1.0) - 1.0)
                             : (1.0 - sqrt(2.0 / T(i) - 1.0));
            }

            return;
        }

        /**
         * @brief get the grad of tau using grad of T
         * 
         * @tparam EIGENVEC 
         * @param tau 
         * @param gradT 
         * @param gradTau 
         */
        template <typename EIGENVEC>
        static inline void backwardGradT(const Eigen::VectorXd &tau,
                                         const Eigen::VectorXd &gradT,
                                         EIGENVEC &gradTau)
        {
            const int sizeTau = tau.size();
            gradTau.resize(sizeTau);
            double denSqrt;
            for (int i = 0; i < sizeTau; i++)
            {
                if (tau(i) > 0)
                {
                    gradTau(i) = gradT(i) * (tau(i) + 1.0);
                }
                else
                {
                    denSqrt = (0.5 * tau(i) - 1.0) * tau(i) + 1.0;
                    gradTau(i) = gradT(i) * (1.0 - tau(i)) / (denSqrt * denSqrt);
                }
            }
            return;
        }

        static inline void forwardV(const Eigen::Matrix3Xd &vEndVelocity,
                                    const Eigen::VectorXd &evi,
                                    Eigen::Vector3d &endv){
            Eigen::VectorXd q = evi.normalized().head(7);
            endv = vEndVelocity.rightCols(7) * q.cwiseProduct(q) + vEndVelocity.col(0);
        }
        /**
         * @brief get mid points(mid points are neccessary for formulating MINCO)
         * 
         * @param xi        the weight of each vector 
         * @param vIdx      the index of corresponding vPolys(path is shortten, some vPolys are abandoned)
         * @param vPolys    the vector expression of polytopes([start vetex, vector_1, ..., vectorn_n)
         * @param P         the out put mid points
         */
        static inline void forwardP(const Eigen::VectorXd &xi,
                                    const Eigen::VectorXi &vIdx,
                                    const PolyhedraV &vPolys,
                                    Eigen::Matrix3Xd &P)
        {
            const int sizeP = vIdx.size();
            P.resize(3, sizeP);
            Eigen::VectorXd q;
            for (int i = 0, j = 0, k, l; i < sizeP; i++, j += k)
            {
                l = vIdx(i);
                k = vPolys[l].cols();
                q = xi.segment(j, k).normalized().head(k - 1);
                P.col(i) = vPolys[l].rightCols(k - 1) * q.cwiseProduct(q) +
                           vPolys[l].col(0);
            }
            return;
        }

        /**
         * @brief the interface of BFGS
         * 
         * @param ptr 
         * @param xi     optimization variable
         * @param gradXi 
         * @return objective cost 
         */
        static inline double costTinyNLS(void *ptr,
                                         const Eigen::VectorXd &xi,
                                         Eigen::VectorXd &gradXi)
        {
            const int n = xi.size();
            const Eigen::Matrix3Xd &ovPoly = *(Eigen::Matrix3Xd *)ptr;

            const double sqrNormXi = xi.squaredNorm();
            const double invNormXi = 1.0 / sqrt(sqrNormXi);
            const Eigen::VectorXd unitXi = xi * invNormXi;
            const Eigen::VectorXd r = unitXi.head(n - 1);
            const Eigen::Vector3d delta = ovPoly.rightCols(n - 1) * r.cwiseProduct(r) +
                                          ovPoly.col(1) - ovPoly.col(0);

            double cost = delta.squaredNorm();
            gradXi.head(n - 1) = (ovPoly.rightCols(n - 1).transpose() * (2 * delta)).array() *
                                 r.array() * 2.0;
            gradXi(n - 1) = 0.0;
            gradXi = (gradXi - unitXi.dot(gradXi) * unitXi).eval() * invNormXi;

            const double sqrNormViolation = sqrNormXi - 1.0;
            if (sqrNormViolation > 0.0)
            {
                double c = sqrNormViolation * sqrNormViolation;
                const double dc = 3.0 * c;
                c *= sqrNormViolation;
                cost += c;
                gradXi += dc * 2.0 * xi; 
            }

            return cost; 
        }

        /**
         * @brief map mid points to weights of polytope vectors
         * since mid points are constrained in convex hulls, while optimizer wish free optimization, square weights are used
         * besides, polytope may not be a simplex，optimization is used in this function
         * 
         * @tparam EIGENVEC 
         * @param P         mid points
         * @param vIdx      index of corresponding polytope
         * @param vPolys    all the polytopes
         * @param xi        weights
         */
        template <typename EIGENVEC>
        static inline void backwardP(const Eigen::Matrix3Xd &P,
                                     const Eigen::VectorXi &vIdx,
                                     const PolyhedraV &vPolys,
                                     EIGENVEC &xi)
        {
            const int sizeP = P.cols();

            double minSqrD;
            lbfgs::lbfgs_parameter_t tiny_nls_params;
            tiny_nls_params.past = 0;
            tiny_nls_params.delta = 1.0e-5;
            tiny_nls_params.g_epsilon = FLT_EPSILON;
            tiny_nls_params.max_iterations = 128;

            Eigen::Matrix3Xd ovPoly;
            for (int i = 0, j = 0, k, l; i < sizeP; i++, j += k)
            {
                l = vIdx(i);
                k = vPolys[l].cols(); 
                //ovPoly: [pathpoint vertex1 ... vertexk]
                ovPoly.resize(3, k + 1);
                ovPoly.col(0) = P.col(i);
                ovPoly.rightCols(k) = vPolys[l];
                Eigen::VectorXd x(k);
                x.setConstant(sqrt(1.0 / k));
                lbfgs::lbfgs_optimize(x,
                                      minSqrD,
                                      &GCOPTER_PolytopeSFC::costTinyNLS,
                                      nullptr,
                                      nullptr,
                                      &ovPoly,
                                      tiny_nls_params);

                xi.segment(j, k) = x;
            }

            return;
        }

        /**
         * @brief get the grad of weights
         * 
         * @tparam EIGENVEC 
         * @param xi        weights 
         * @param vIdx      index of corresponding polytope
         * @param vPolys    all the polytopes
         * @param gradP     grad of mid points
         * @param gradXi    grad of weights
         */
        template <typename EIGENVEC>
        static inline void backwardGradP(const Eigen::VectorXd &xi,
                                         const Eigen::VectorXi &vIdx,
                                         const PolyhedraV &vPolys,
                                         const Eigen::Matrix3Xd &gradP,
                                         EIGENVEC &gradXi)
        {
            const int sizeP = vIdx.size();
            gradXi.resize(xi.size());

            double normInv;
            Eigen::VectorXd q, gradQ, unitQ;
            for (int i = 0, j = 0, k, l; i < sizeP; i++, j += k)
            {
                l = vIdx(i);
                k = vPolys[l].cols();
                q = xi.segment(j, k);
                normInv = 1.0 / q.norm();
                unitQ = q * normInv;
                gradQ.resize(k);
                gradQ.head(k - 1) = (vPolys[l].rightCols(k - 1).transpose() * gradP.col(i)).array() *
                                    unitQ.head(k - 1).array() * 2.0;
                gradQ(k - 1) = 0.0;
                gradXi.segment(j, k) = (gradQ - unitQ * unitQ.dot(gradQ)) * normInv;
            }

            return;
        }

        template <typename EIGENVEC>
        static inline void backwardGradV(const Eigen::VectorXd &evi,
                                         const Eigen::Matrix3Xd &vEndVelocity,
                                         const Eigen::VectorXd &gradv,
                                         EIGENVEC &gradEvi){
            gradEvi.resize(evi.size());
            Eigen::VectorXd q, gradQ, unitQ;
            double normInv;
            int k = vEndVelocity.cols();
            q = evi;
            normInv = 1.0 / q.norm();
            unitQ = q * normInv;
            gradQ.resize(k);
            gradQ.head(k - 1) = (vEndVelocity.rightCols(k - 1).transpose() * gradv).array() *
                                unitQ.head(k - 1).array() * 2.0;
            gradQ(k - 1) = 0.0;
            gradEvi = (gradQ - unitQ * unitQ.dot(gradQ)) * normInv;
        }

        /**
         * @brief a special cost function,  stabilize the norm of xi at 1 
         * 
         * @tparam EIGENVEC 
         * @param xi 
         * @param vIdx 
         * @param vPolys 
         * @param cost 
         * @param gradXi 
         */
        template <typename EIGENVEC>
        static inline void normRetrictionLayer(const Eigen::VectorXd &xi,
                                               const Eigen::VectorXi &vIdx,
                                               const PolyhedraV &vPolys,
                                               double &cost,
                                               EIGENVEC &gradXi)
        {
            const int sizeP = vIdx.size();
            gradXi.resize(xi.size());

            double sqrNormQ, sqrNormViolation, c, dc;
            Eigen::VectorXd q;
            for (int i = 0, j = 0, k; i < sizeP; i++, j += k)
            {
                k = vPolys[vIdx(i)].cols();

                q = xi.segment(j, k);
                sqrNormQ = q.squaredNorm();
                sqrNormViolation = sqrNormQ - 1.0;
                if (sqrNormViolation > 0.0)
                {
                    c = sqrNormViolation * sqrNormViolation;
                    dc = 3.0 * c;
                    c *= sqrNormViolation;
                    cost += c;
                    gradXi.segment(j, k) += dc * 2.0 * q;
                }
            }

            return;
        }


        template <typename EIGENVEC>
        static inline void normRetrictionLayerV(const Eigen::VectorXd &evi,
                                               double &cost,
                                               EIGENVEC &gradEvi)
        {
            double sqrNormQ, sqrNormViolation, c, dc;
            Eigen::VectorXd q;
            q = evi;
            sqrNormQ = q.squaredNorm();
            sqrNormViolation = sqrNormQ - 1.0;
            if (sqrNormViolation > 0.0)
            {
                c = sqrNormViolation * sqrNormViolation;
                dc = 3.0 * c;
                c *= sqrNormViolation;
                cost += c;
                gradEvi += dc * 2.0 * q;
            }

            return;
        }

        /**
         * @brief a smooth punish function, punish if x > mu
         * 
         * @param x 
         * @param mu    the punish threshold
         * @param f     punish cost
         * @param df    grad of x
         * @return true 
         * @return false 
         */
        static inline bool smoothedL1(const double &x,
                                      const double &mu,
                                      double &f,
                                      double &df)
        {
            if (x < 0.0)
            {
                return false;
            }
            else if (x > mu)
            {
                f = x - 0.5 * mu;
                df = 1.0;
                return true;
            }
            else
            {
                const double xdmu = x / mu;
                const double sqrxdmu = xdmu * xdmu;
                const double mumxd2 = mu - 0.5 * x;
                f = mumxd2 * sqrxdmu * xdmu;
                df = sqrxdmu * ((-0.5) * xdmu + 3.0 * mumxd2 / mu);
                return true;
            }
        }

        /**
         * @brief user define discrete cost. for inequality constraint 
         * and integral functions that are difficult to be expressed analytically 
         * 
         * @param T                     intervals
         * @param coeffs                polynomial params
         * @param hIdx                  index of corresponding polytopes
         * @param hPolys                all the polytopes
         * @param smoothFactor          the mu in smoothedL1
         * @param integralResolution    the number of discrete pieces
         * @param magnitudeBounds       user define, here [v_max, a_max,]^T 
         * @param penaltyWeights        [pos_weight, vel_weight, acc_weight]^T
         * @param cost                  objective cost
         * @param gradT                 grad of intervals
         * @param gradC                 grad of coeffs
         */
        static inline void attachPenaltyFunctional(const Eigen::VectorXd &T,
                                                const Eigen::MatrixX3d &coeffs,
                                                const Eigen::VectorXi &hIdx,
                                                const PolyhedraH &hPolys,
                                                const double &smoothFactor,
                                                const int &integralResolution,
                                                const Eigen::VectorXd &magnitudeBounds,
                                                const Eigen::VectorXd &penaltyWeights,
                                                double &cost,
                                                Eigen::VectorXd &gradT,
                                                Eigen::MatrixX3d &gradC,
                                                std::vector<FastCheckTraj> &swarmTr){
            const double velSqrMax = magnitudeBounds(0) * magnitudeBounds(0);
            const double accSqrMax = magnitudeBounds(1) * magnitudeBounds(1);
            const double jerSqrMax = magnitudeBounds(3) * magnitudeBounds(3);
            const double SwarmAvoidMax = magnitudeBounds(2) * magnitudeBounds(2);

            const double weightPos = penaltyWeights(0);
            const double weightVel = penaltyWeights(1);
            const double weightAcc = penaltyWeights(2);
            const double weightJer = penaltyWeights(4);
            const double weightSwarm = penaltyWeights(3);

            Eigen::Vector3d pos, vel, acc, jer, sna, sw_pos;
            Eigen::Vector3d gradPos, gradVel, gradAcc, gradJer;

            double step, alpha;
            double s1, s2, s3, s4, s5, s_cur;
            Eigen::Matrix<double, 6, 1> beta0, beta1, beta2, beta3, beta4;
            Eigen::Vector3d outerNormal;
            Eigen::Vector3d swarmNormal;
            int K, L;
            double node, pena;
            double violaPos, violaVel, violaAcc, violaSwarm, violaJer;
            double violaPosPena, violaVelPena, violaAccPena, violaJerPena, violaSwarmPena;
            double violaPosPenaD, violaVelPenaD, violaAccPenaD, violaJerPenaD, violaSwarmPenaD;

            const int pieceNum = T.size();
            const double integralFrac = 1.0 / integralResolution;
            s_cur = 0;
            for (int i = 0; i < pieceNum; i++)
            {
                const Eigen::Matrix<double, 6, 3> &c = coeffs.block<6, 3>(i * 6, 0);
                step = T(i) * integralFrac;
                for (int j = 0; j <= integralResolution; j++)
                {
                    s1 = j * step;
                    s2 = s1 * s1;
                    s3 = s2 * s1;
                    s4 = s2 * s2;
                    s5 = s4 * s1;
                    beta0(0) = 1.0, beta0(1) = s1, beta0(2) = s2, beta0(3) = s3, beta0(4) = s4, beta0(5) = s5;
                    beta1(0) = 0.0, beta1(1) = 1.0, beta1(2) = 2.0 * s1, beta1(3) = 3.0 * s2, beta1(4) = 4.0 * s3, beta1(5) = 5.0 * s4;
                    beta2(0) = 0.0, beta2(1) = 0.0, beta2(2) = 2.0, beta2(3) = 6.0 * s1, beta2(4) = 12.0 * s2, beta2(5) = 20.0 * s3;
                    beta3(0) = 0.0, beta3(1) = 0.0, beta3(2) = 0.0, beta3(3) = 6.0, beta3(4) = 24.0 * s1, beta3(5) = 60.0 * s2;
                    beta4(0) = 0.0, beta4(1) = 0.0, beta4(2) = 0.0, beta4(3) = 0.0, beta4(4) = 24.0, beta4(5) = 120.0 * s1;
                    pos = c.transpose() * beta0;
                    vel = c.transpose() * beta1;
                    acc = c.transpose() * beta2;
                    jer = c.transpose() * beta3;
                    sna = c.transpose() * beta4;

                    violaVel = vel.squaredNorm() - velSqrMax;
                    violaAcc = acc.squaredNorm() - accSqrMax;
                    violaJer = jer.squaredNorm() - jerSqrMax;

                    L = hIdx(i);
                    K = hPolys[L].rows();

                    gradPos.setZero(), gradVel.setZero(), gradAcc.setZero(), gradJer.setZero();
                    pena = 0.0;

                    for (int k = 0; k < K; k++)
                    {
                        outerNormal = hPolys[L].block<1, 3>(k, 0);
                        violaPos = outerNormal.dot(pos) + hPolys[L](k, 3);
                        if (smoothedL1(violaPos, smoothFactor, violaPosPena, violaPosPenaD))
                        {
                            gradPos += weightPos * violaPosPenaD * outerNormal;
                            pena += weightPos * violaPosPena;
                        }
                    }
                    if (smoothedL1(violaVel, smoothFactor, violaVelPena, violaVelPenaD))
                    {
                        gradVel += weightVel * violaVelPenaD * 2.0 * vel;
                        pena += weightVel * violaVelPena;
                    }
                    if (smoothedL1(violaAcc, smoothFactor, violaAccPena, violaAccPenaD))
                    {
                        gradAcc += weightAcc * violaAccPenaD * 2.0 * acc;
                        pena += weightAcc * violaAccPena;
                    }
                    if (smoothedL1(violaJer, smoothFactor, violaJerPena, violaJerPenaD))
                    {
                        gradJer += weightJer * violaJerPenaD * 2.0 * jer;
                        pena += weightJer * violaJerPena;
                    }
                    /* swarm viola */
                    for(auto &swtr : swarmTr){
                        if(swtr.GetPos(s_cur + s1, sw_pos)){
                            swarmNormal = (sw_pos - pos);
                            violaSwarm = SwarmAvoidMax - swarmNormal.squaredNorm();
                            if (smoothedL1(violaSwarm, smoothFactor, violaSwarmPena, violaSwarmPenaD))
                            {   
                                gradPos += weightSwarm * violaSwarmPenaD * swarmNormal * 2.0;
                                pena += weightSwarm * violaSwarmPena;
                            }
                        }
                    }

                    node = (j == 0 || j == integralResolution) ? 0.5 : 1.0;
                    alpha = j * integralFrac;
                    gradC.block<6, 3>(i * 6, 0) += (beta0 * gradPos.transpose() +
                                                    beta1 * gradVel.transpose() +
                                                    beta2 * gradAcc.transpose() +
                                                    beta3 * gradJer.transpose()) *
                                                   node * step;
                    gradT(i) += (gradPos.dot(vel) +
                                 gradVel.dot(acc) +
                                 gradAcc.dot(jer) + 
                                 gradJer.dot(sna)) *
                                    alpha * node * step +
                                node * integralFrac * pena;
                    cost += node * step * pena;
                }
                s_cur += T(i);
            }
            return;
        }


        // magnitudeBounds = [v_max, omg_max, theta_max, thrust_min, thrust_max]^T
        // penaltyWeights = [pos_weight, vel_weight, omg_weight, theta_weight, thrust_weight]^T
        // physicalParams = [vehicle_mass, gravitational_acceleration, horitonral_drag_coeff,
        //                   vertical_drag_coeff, parasitic_drag_coeff, speed_smooth_factor]^T
        // static inline void attachPenaltyFunctional(const Eigen::VectorXd &T,
        //                                            const Eigen::MatrixX3d &coeffs,
        //                                            const Eigen::VectorXi &hIdx,
        //                                            const PolyhedraH &hPolys,
        //                                            const double &smoothFactor,
        //                                            const int &integralResolution,
        //                                            const Eigen::VectorXd &magnitudeBounds,
        //                                            const Eigen::VectorXd &penaltyWeights,
        //                                            flatness::FlatnessMap &flatMap,
        //                                            double &cost,
        //                                            Eigen::VectorXd &gradT,
        //                                            Eigen::MatrixX3d &gradC)
        // {
        //     const double velSqrMax = magnitudeBounds(0) * magnitudeBounds(0);
        //     const double omgSqrMax = magnitudeBounds(1) * magnitudeBounds(1);
        //     const double thetaMax = magnitudeBounds(2);
        //     const double thrustMean = 0.5 * (magnitudeBounds(3) + magnitudeBounds(4));
        //     const double thrustRadi = 0.5 * fabs(magnitudeBounds(4) - magnitudeBounds(3));
        //     const double thrustSqrRadi = thrustRadi * thrustRadi;

        //     const double weightPos = penaltyWeights(0);
        //     const double weightVel = penaltyWeights(1);
        //     const double weightOmg = penaltyWeights(2);
        //     const double weightTheta = penaltyWeights(3);
        //     const double weightThrust = penaltyWeights(4);

        //     Eigen::Vector3d pos, vel, acc, jer, sna;
        //     Eigen::Vector3d totalGradPos, totalGradVel, totalGradAcc, totalGradJer;
        //     double totalGradPsi, totalGradPsiD;
        //     double thr, cos_theta;
        //     Eigen::Vector4d quat;
        //     Eigen::Vector3d omg;
        //     double gradThr;
        //     Eigen::Vector4d gradQuat;
        //     Eigen::Vector3d gradPos, gradVel, gradOmg;

        //     double step, alpha;
        //     double s1, s2, s3, s4, s5;
        //     Eigen::Matrix<double, 6, 1> beta0, beta1, beta2, beta3, beta4;
        //     Eigen::Vector3d outerNormal;
        //     int K, L;
        //     double violaPos, violaVel, violaOmg, violaTheta, violaThrust;
        //     double violaPosPenaD, violaVelPenaD, violaOmgPenaD, violaThetaPenaD, violaThrustPenaD;
        //     double violaPosPena, violaVelPena, violaOmgPena, violaThetaPena, violaThrustPena;
        //     double node, pena;

        //     const int pieceNum = T.size();
        //     const double integralFrac = 1.0 / integralResolution;
        //     for (int i = 0; i < pieceNum; i++)
        //     {
        //         const Eigen::Matrix<double, 6, 3> &c = coeffs.block<6, 3>(i * 6, 0);
        //         step = T(i) * integralFrac;
        //         for (int j = 0; j <= integralResolution; j++)
        //         {
        //             s1 = j * step;
        //             s2 = s1 * s1;
        //             s3 = s2 * s1;
        //             s4 = s2 * s2;
        //             s5 = s4 * s1;
        //             beta0(0) = 1.0, beta0(1) = s1, beta0(2) = s2, beta0(3) = s3, beta0(4) = s4, beta0(5) = s5;
        //             beta1(0) = 0.0, beta1(1) = 1.0, beta1(2) = 2.0 * s1, beta1(3) = 3.0 * s2, beta1(4) = 4.0 * s3, beta1(5) = 5.0 * s4;
        //             beta2(0) = 0.0, beta2(1) = 0.0, beta2(2) = 2.0, beta2(3) = 6.0 * s1, beta2(4) = 12.0 * s2, beta2(5) = 20.0 * s3;
        //             beta3(0) = 0.0, beta3(1) = 0.0, beta3(2) = 0.0, beta3(3) = 6.0, beta3(4) = 24.0 * s1, beta3(5) = 60.0 * s2;
        //             beta4(0) = 0.0, beta4(1) = 0.0, beta4(2) = 0.0, beta4(3) = 0.0, beta4(4) = 24.0, beta4(5) = 120.0 * s1;
        //             pos = c.transpose() * beta0;
        //             vel = c.transpose() * beta1;
        //             acc = c.transpose() * beta2;
        //             jer = c.transpose() * beta3;
        //             sna = c.transpose() * beta4;

        //             flatMap.forward(vel, acc, jer, 0.0, 0.0, thr, quat, omg);

        //             violaVel = vel.squaredNorm() - velSqrMax;
        //             violaOmg = omg.squaredNorm() - omgSqrMax;
        //             cos_theta = 1.0 - 2.0 * (quat(1) * quat(1) + quat(2) * quat(2));
        //             violaTheta = acos(cos_theta) - thetaMax;
        //             violaThrust = (thr - thrustMean) * (thr - thrustMean) - thrustSqrRadi;

        //             gradThr = 0.0;
        //             gradQuat.setZero();
        //             gradPos.setZero(), gradVel.setZero(), gradOmg.setZero();
        //             pena = 0.0;

        //             L = hIdx(i);
        //             K = hPolys[L].rows();
        //             for (int k = 0; k < K; k++)
        //             {
        //                 outerNormal = hPolys[L].block<1, 3>(k, 0);
        //                 violaPos = outerNormal.dot(pos) + hPolys[L](k, 3);
        //                 if (smoothedL1(violaPos, smoothFactor, violaPosPena, violaPosPenaD))
        //                 {
        //                     gradPos += weightPos * violaPosPenaD * outerNormal;
        //                     pena += weightPos * violaPosPena;
        //                 }
        //             }

        //             if (smoothedL1(violaVel, smoothFactor, violaVelPena, violaVelPenaD))
        //             {
        //                 gradVel += weightVel * violaVelPenaD * 2.0 * vel;
        //                 pena += weightVel * violaVelPena;
        //             }

        //             if (smoothedL1(violaOmg, smoothFactor, violaOmgPena, violaOmgPenaD))
        //             {
        //                 gradOmg += weightOmg * violaOmgPenaD * 2.0 * omg;
        //                 pena += weightOmg * violaOmgPena;
        //             }

        //             if (smoothedL1(violaTheta, smoothFactor, violaThetaPena, violaThetaPenaD))
        //             {
        //                 gradQuat += weightTheta * violaThetaPenaD /
        //                             sqrt(1.0 - cos_theta * cos_theta) * 4.0 *
        //                             Eigen::Vector4d(0.0, quat(1), quat(2), 0.0);
        //                 pena += weightTheta * violaThetaPena;
        //             }

        //             if (smoothedL1(violaThrust, smoothFactor, violaThrustPena, violaThrustPenaD))
        //             {
        //                 gradThr += weightThrust * violaThrustPenaD * 2.0 * (thr - thrustMean);
        //                 pena += weightThrust * violaThrustPena;
        //             }

        //             flatMap.backward(gradPos, gradVel, gradThr, gradQuat, gradOmg,
        //                              totalGradPos, totalGradVel, totalGradAcc, totalGradJer,
        //                              totalGradPsi, totalGradPsiD);

        //             node = (j == 0 || j == integralResolution) ? 0.5 : 1.0;
        //             alpha = j * integralFrac;
        //             gradC.block<6, 3>(i * 6, 0) += (beta0 * totalGradPos.transpose() +
        //                                             beta1 * totalGradVel.transpose() +
        //                                             beta2 * totalGradAcc.transpose() +
        //                                             beta3 * totalGradJer.transpose()) *
        //                                            node * step;
        //             gradT(i) += (totalGradPos.dot(vel) +
        //                          totalGradVel.dot(acc) +
        //                          totalGradAcc.dot(jer) +
        //                          totalGradJer.dot(sna)) *
        //                             alpha * node * step +
        //                         node * integralFrac * pena;
        //             cost += node * step * pena;
        //         }
        //     }

        //     return;
        // }

        /**
         * @brief interface of BFGS, optimize MINCO
         * 
         * @param ptr 
         * @param x 
         * @param g 
         * @return objective cost
         */
        static inline double costFunctional(void *ptr,
                                            const Eigen::VectorXd &x,
                                            Eigen::VectorXd &g)
        {
            GCOPTER_PolytopeSFC &obj = *(GCOPTER_PolytopeSFC *)ptr;
            const int dimTau = obj.temporalDim;
            const int dimXi = obj.spatialDim;
            const int dimEvi = obj.endVelDim;

            const double weightT = obj.rho;
            const double weightminT = obj.phi;
            double fminT, dfminT;
            Eigen::Map<const Eigen::VectorXd> tau(x.data(), dimTau);
            Eigen::Map<const Eigen::VectorXd> xi(x.data() + dimTau, dimXi);
            Eigen::Map<const Eigen::VectorXd> evi(x.data() + dimTau + dimXi, dimEvi);

            Eigen::Map<Eigen::VectorXd> gradTau(g.data(), dimTau);
            Eigen::Map<Eigen::VectorXd> gradXi(g.data() + dimTau, dimXi);
            Eigen::Map<Eigen::VectorXd> gradEvi(g.data() + dimTau + dimXi, dimEvi);

            Eigen::Vector3d endv;
            forwardV(obj.vEndVel, evi, endv);
            forwardT(tau, obj.times);
            forwardP(xi, obj.vPolyIdx, obj.vPolytopes, obj.points);

            double cost;
            obj.minco.setEndVel(endv);
            obj.minco.setParameters(obj.points, obj.times);
            obj.minco.getEnergy(cost);
            obj.minco.getEnergyPartialGradByCoeffs(obj.partialGradByCoeffs);
            obj.minco.getEnergyPartialGradByTimes(obj.partialGradByTimes);
            cost *= 0.1;
            obj.partialGradByCoeffs *= 0.1;
            obj.partialGradByTimes *= 0.1;


            attachPenaltyFunctional(obj.times, obj.minco.getCoeffs(),
                                    obj.hPolyIdx, obj.hPolytopes,
                                    obj.smoothEps, obj.integralRes,
                                    obj.magnitudeBd, obj.penaltyWt,
                                    cost, obj.partialGradByTimes, 
                                    obj.partialGradByCoeffs, obj.swarm_trajs_);

            // obj.minco.propogateGrad(obj.partialGradByCoeffs, obj.partialGradByTimes,
            //                         obj.gradByPoints, obj.gradByTimes);

            obj.minco.propogateGrad(obj.partialGradByCoeffs, obj.partialGradByTimes,
                                    obj.gradByPoints, obj.gradByEndVel, obj.gradByTimes);

            double viola_t = obj.minT - obj.times.sum(); 
            if(smoothedL1(viola_t, 0.01, fminT, dfminT)){
                cost += weightminT * fminT;
                obj.gradByTimes.array() -= dfminT * weightminT;
            }

            cost += weightT * obj.times.sum();
            obj.gradByTimes.array() += weightT;

            backwardGradT(tau, obj.gradByTimes, gradTau);
            backwardGradP(xi, obj.vPolyIdx, obj.vPolytopes, obj.gradByPoints, gradXi);
            backwardGradV(evi, obj.vEndVel, obj.gradByEndVel, gradEvi);

            normRetrictionLayer(xi, obj.vPolyIdx, obj.vPolytopes, cost, gradXi);
            normRetrictionLayerV(evi, cost, gradEvi);

            return cost;
        }

        /**
         * @brief interface of BFGS, shortten the path
         * 
         * @param ptr 
         * @param xi 
         * @param gradXi 
         * @return objective cost 
         */
        static inline double costDistance(void *ptr,
                                          const Eigen::VectorXd &xi,
                                          Eigen::VectorXd &gradXi)
        {
            void **dataPtrs = (void **)ptr;
            const double &dEps = *((const double *)(dataPtrs[0]));
            const Eigen::Vector3d &ini = *((const Eigen::Vector3d *)(dataPtrs[1]));
            const Eigen::Vector3d &fin = *((const Eigen::Vector3d *)(dataPtrs[2]));
            const PolyhedraV &vPolys = *((PolyhedraV *)(dataPtrs[3]));

            double cost = 0.0;
            const int overlaps = vPolys.size() / 2;

            Eigen::Matrix3Xd gradP = Eigen::Matrix3Xd::Zero(3, overlaps);
            Eigen::Vector3d a, b, d;
            Eigen::VectorXd r;
            double smoothedDistance;
            for (int i = 0, j = 0, k = 0; i <= overlaps; i++, j += k)
            {
                a = i == 0 ? ini : b;
                if (i < overlaps)
                {
                    k = vPolys[2 * i + 1].cols();
                    Eigen::Map<const Eigen::VectorXd> q(xi.data() + j, k);
                    r = q.normalized().head(k - 1);
                    b = vPolys[2 * i + 1].rightCols(k - 1) * r.cwiseProduct(r) +
                        vPolys[2 * i + 1].col(0);
                }
                else
                {
                    b = fin;
                }

                d = b - a;
                smoothedDistance = sqrt(d.squaredNorm() + dEps);
                cost += smoothedDistance;

                if (i < overlaps)
                {
                    gradP.col(i) += d / smoothedDistance;
                }
                if (i > 0)
                {
                    gradP.col(i - 1) -= d / smoothedDistance;
                }
            }

            Eigen::VectorXd unitQ;
            double sqrNormQ, invNormQ, sqrNormViolation, c, dc;
            for (int i = 0, j = 0, k; i < overlaps; i++, j += k)
            {
                k = vPolys[2 * i + 1].cols();
                Eigen::Map<const Eigen::VectorXd> q(xi.data() + j, k);
                Eigen::Map<Eigen::VectorXd> gradQ(gradXi.data() + j, k);
                sqrNormQ = q.squaredNorm();
                invNormQ = 1.0 / sqrt(sqrNormQ);
                unitQ = q * invNormQ;
                gradQ.head(k - 1) = (vPolys[2 * i + 1].rightCols(k - 1).transpose() * gradP.col(i)).array() *
                                    unitQ.head(k - 1).array() * 2.0;
                gradQ(k - 1) = 0.0;


                gradQ = (gradQ - unitQ * unitQ.dot(gradQ)).eval() * invNormQ;

                sqrNormViolation = sqrNormQ - 1.0;
                if (sqrNormViolation > 0.0)
                {
                    c = sqrNormViolation * sqrNormViolation;
                    dc = 3.0 * c;
                    c *= sqrNormViolation;
                    cost += c;
                    gradQ += dc * 2.0 * q;
                }
            }

            return cost;
        }

        /**
         * @brief shortten the path
         * 
         * @param ini 
         * @param fin 
         * @param vPolys 
         * @param smoothD 
         * @param path 
         */
        static inline void getShortestPath(const Eigen::Vector3d &ini,
                                           const Eigen::Vector3d &fin,
                                           const PolyhedraV &vPolys,
                                           const double &smoothD,
                                           Eigen::Matrix3Xd &path)
        {
            const int overlaps = vPolys.size() / 2;
            Eigen::VectorXi vSizes(overlaps);
            for (int i = 0; i < overlaps; i++)
            {
                vSizes(i) = vPolys[2 * i + 1].cols();
            }
            Eigen::VectorXd xi(vSizes.sum());
            for (int i = 0, j = 0; i < overlaps; i++)
            {
                xi.segment(j, vSizes(i)).setConstant(sqrt(1.0 / vSizes(i)));
                j += vSizes(i);
            }

            double minDistance;
            void *dataPtrs[4];
            dataPtrs[0] = (void *)(&smoothD);
            dataPtrs[1] = (void *)(&ini);
            dataPtrs[2] = (void *)(&fin);
            dataPtrs[3] = (void *)(&vPolys);
            lbfgs::lbfgs_parameter_t shortest_path_params;
            shortest_path_params.past = 3;
            shortest_path_params.delta = 1.0e-3;
            shortest_path_params.g_epsilon = 1.0e-5;

            lbfgs::lbfgs_optimize(xi,
                                  minDistance,
                                  &GCOPTER_PolytopeSFC::costDistance,
                                  nullptr,
                                  nullptr,
                                  dataPtrs,
                                  shortest_path_params);

            path.resize(3, overlaps + 2);
            path.leftCols<1>() = ini;
            path.rightCols<1>() = fin;
            Eigen::VectorXd r;
            for (int i = 0, j = 0, k; i < overlaps; i++, j += k)
            {
                k = vPolys[2 * i + 1].cols();
                Eigen::Map<const Eigen::VectorXd> q(xi.data() + j, k);
                r = q.normalized().head(k - 1);
                path.col(i + 1) = vPolys[2 * i + 1].rightCols(k - 1) * r.cwiseProduct(r) +
                                  vPolys[2 * i + 1].col(0);
            }

            return;
        }

        /**
         * @brief get origin and vectors of each corridor. 
         * for quick hull corridors, not used here.
         * 
         * @param hPs ieqs of polytopes
         * @param vPs corridori: [origin, dir1, dir2....]
         * @return true 
         * @return false 
         */
        static inline bool processCorridor(const PolyhedraH &hPs,
                                           PolyhedraV &vPs)
        {
            const int sizeCorridor = hPs.size() - 1;

            vPs.clear();
            vPs.reserve(2 * sizeCorridor + 1);

            int nv;
            PolyhedronH curIH;
            PolyhedronV curIV, curIOB;
            for (int i = 0; i < sizeCorridor; i++)
            {
                if (!geo_utils::enumerateVs(hPs[i], curIV))
                {
                    return false;
                }
                nv = curIV.cols();
                curIOB.resize(3, nv);
                curIOB.col(0) = curIV.col(0);
                curIOB.rightCols(nv - 1) = curIV.rightCols(nv - 1).colwise() - curIV.col(0);
                vPs.push_back(curIOB);

                curIH.resize(hPs[i].rows() + hPs[i + 1].rows(), 4);
                curIH.topRows(hPs[i].rows()) = hPs[i];
                curIH.bottomRows(hPs[i + 1].rows()) = hPs[i + 1];
                if (!geo_utils::enumerateVs(curIH, curIV))
                {
                    return false;
                }
                nv = curIV.cols();
                curIOB.resize(3, nv);
                curIOB.col(0) = curIV.col(0);
                curIOB.rightCols(nv - 1) = curIV.rightCols(nv - 1).colwise() - curIV.col(0);
                vPs.push_back(curIOB);
            }

            if (!geo_utils::enumerateVs(hPs.back(), curIV))
            {
                return false;
            }
            nv = curIV.cols();
            curIOB.resize(3, nv);
            curIOB.col(0) = curIV.col(0);
            curIOB.rightCols(nv - 1) = curIV.rightCols(nv - 1).colwise() - curIV.col(0);
            vPs.push_back(curIOB);

            return true;
        }

        /**
         * @brief set the initial params 
         * 
         * @param path          initial path
         * @param speed         for init T
         * @param intervalNs    index of corresponding polytops
         * @param innerPoints   
         * @param timeAlloc 
         */
        static inline void setInitial(const Eigen::Matrix3Xd &path,
                                      const double &speed,
                                      const Eigen::VectorXi &intervalNs,
                                      Eigen::Matrix3Xd &innerPoints,
                                      Eigen::VectorXd &timeAlloc)
        {
            const int sizeM = intervalNs.size();
            const int sizeN = intervalNs.sum();
            innerPoints.resize(3, sizeN - 1);
            timeAlloc.resize(sizeN);

            Eigen::Vector3d a, b, c;
            for (int i = 0, j = 0, k = 0, l; i < sizeM; i++)
            {
                l = intervalNs(i);
                a = path.col(i);
                b = path.col(i + 1);
                c = (b - a) / l;
                timeAlloc.segment(j, l).setConstant(c.norm() / speed);
                j += l;
                for (int m = 0; m < l; m++)
                {
                    if (i > 0 || m > 0)
                    {
                        innerPoints.col(k++) = a + c * m;
                    }
                }
            }
        }

    public:

        /**
         * @brief set path, corridors and optimization params
         * 
         */
        inline bool setup(const double &timeWeight,
                          const double &mintimeWeight,
                          const double &mintime,
                          const Eigen::Matrix3d &initialPVA,
                          const Eigen::Matrix3d &terminalPVA,
                          const PolyhedraH &safeCorridor,
                          const PolyhedraV &CorridorV,
                          const double &lengthPerPiece,
                          const double &smoothingFactor,
                          const int &integralResolution,
                          const Eigen::VectorXd &magnitudeBounds,
                          const Eigen::VectorXd &penaltyWeights,
                          std::vector<FastCheckTraj> &swarm_trajs)
                        //   const Eigen::VectorXd &physicalParams)
        {
            swarm_trajs_ = swarm_trajs;
            rho = timeWeight;
            phi = mintimeWeight;
            minT = mintime;
            headPVA = initialPVA;
            tailPVA = terminalPVA;

            hPolytopes = safeCorridor;
            for (size_t i = 0; i < hPolytopes.size(); i++)
            {
                const Eigen::ArrayXd norms =
                    hPolytopes[i].leftCols<3>().rowwise().norm();
                hPolytopes[i].array().colwise() /= norms;
            }
            vPolytopes = CorridorV;

            //for only hPolytopes input
            // if (!processCorridor(hPolytopes, vPolytopes))
            // {
            //     return false;
            // }

            polyN = hPolytopes.size();
            smoothEps = smoothingFactor;
            integralRes = integralResolution;
            magnitudeBd = magnitudeBounds;
            penaltyWt = penaltyWeights;
            // physicalPm = physicalParams;
            allocSpeed = magnitudeBd(0) * 1.5;

            getShortestPath(headPVA.col(0), tailPVA.col(0),
                            vPolytopes, smoothEps, shortPath);
            const Eigen::Matrix3Xd deltas = shortPath.rightCols(polyN) - shortPath.leftCols(polyN);
            pieceIdx = (deltas.colwise().norm() / lengthPerPiece).cast<int>().transpose();
            pieceIdx.array() += 1;
            pieceN = pieceIdx.sum();

            temporalDim = pieceN;
            spatialDim = 0;
            vPolyIdx.resize(pieceN - 1);
            hPolyIdx.resize(pieceN);
            for (int i = 0, j = 0, k; i < polyN; i++)
            {
                k = pieceIdx(i);
                for (int l = 0; l < k; l++, j++)
                {
                    if (l < k - 1)
                    {
                        vPolyIdx(j) = 2 * i;
                        spatialDim += vPolytopes[2 * i].cols();
                    }
                    else if (i < polyN - 1)
                    {
                        vPolyIdx(j) = 2 * i + 1;
                        spatialDim += vPolytopes[2 * i + 1].cols();
                    }
                    hPolyIdx(j) = i;
                }
            }

            endVelDim = 0;
            // std::cout<<"Fixsetup4.5"<<std::endl;
            endVelDim = 8;
            vEndVel.resize(3, 8);
            Eigen::Matrix<double, 3, 2> bound;
            Eigen::Vector3d vmax, vmin; 
            for(int i = 0; i < 3; i++){
                bound(i, 0) = -hPolytopes.back()(i*2, 3) - tailPVA(i, 0); // up
                bound(i, 0) = std::max(0.001, bound(i, 0));
                vmax(i) = std::min(sqrt(2.0 * bound(i, 0) * magnitudeBounds[1] * 0.8), magnitudeBounds[0] * 0.8);
                bound(i, 1) = hPolytopes.back()(i*2 + 1, 3) - tailPVA(i, 0); //down
                bound(i, 1) = std::min(-0.001, bound(i, 1));
                vmin(i) = -std::min(sqrt(-2.0 * bound(i, 1) * magnitudeBounds[1] * 0.8), magnitudeBounds[0] * 0.8);
            }

            // std::cout<<"bound:"<<bound<<std::endl;
            // std::cout<<"vmin:"<<vmin.transpose()<<std::endl;
            // std::cout<<"vmax:"<<vmax.transpose()<<std::endl;

            for(int dim1 = 0; dim1 <= 1; dim1++){
                for(int dim2 = 0; dim2 <= 1; dim2++){
                    for(int dim3 = 0; dim3 <= 1; dim3++){
                        vEndVel(0, 4*dim3 + 2*dim2 + dim1) = dim1 ? vmax(0) : vmin(0);
                        vEndVel(1, 4*dim3 + 2*dim2 + dim1) = dim2 ? vmax(1) : vmin(1);
                        vEndVel(2, 4*dim3 + 2*dim2 + dim1) = dim3 ? vmax(2) : vmin(2);
                    }
                }
            }
            for(int j = 1; j < 8; j++){
                vEndVel.col(j) = vEndVel.col(j) - vEndVel.col(0);
            }

            // Setup for MINCO_S3NU, FlatnessMap, and L-BFGS solver
            minco.setConditions(headPVA, tailPVA, pieceN);
            // flatmap.reset(physicalPm(0), physicalPm(1), physicalPm(2),
            //               physicalPm(3), physicalPm(4), physicalPm(5));

            // Allocate temp variables
            points.resize(3, pieceN - 1);
            times.resize(pieceN);
            gradByPoints.resize(3, pieceN - 1);
            gradByTimes.resize(pieceN);
            partialGradByCoeffs.resize(6 * pieceN, 3);
            partialGradByTimes.resize(pieceN);

            return true;
        }

        /**
         * @brief call after setup()
         * 
         * @param traj       the optimized traj   
         * @param relCostTol 
         * @return double 
         */
        inline double optimize(Trajectory<5> &traj,
                               const double &relCostTol)
        {
            Eigen::VectorXd x(temporalDim + spatialDim + endVelDim);
            Eigen::Map<Eigen::VectorXd> tau(x.data(), temporalDim);
            Eigen::Map<Eigen::VectorXd> xi(x.data() + temporalDim, spatialDim);
            Eigen::Map<Eigen::VectorXd> evi(x.data() + temporalDim + spatialDim, endVelDim);

            //points are downsampled points of shortPath 
            setInitial(shortPath, allocSpeed, pieceIdx, points, times);
            double tsum = times.sum();
            times = times * std::max(tsum, minT) / tsum;
            backwardT(times, tau);
            backwardP(points, vPolyIdx, vPolytopes, xi);
            evi.setOnes();

            double minCostFunctional;
            lbfgs_params.mem_size = 256;
            lbfgs_params.past = 3;
            lbfgs_params.min_step = 1.0e-32;
            lbfgs_params.g_epsilon = 0.0;
            lbfgs_params.delta = relCostTol;

            int ret = lbfgs::lbfgs_optimize(x,
                                            minCostFunctional,
                                            &GCOPTER_PolytopeSFC::costFunctional,
                                            nullptr,
                                            nullptr,
                                            this,
                                            lbfgs_params);

            if (ret >= 0)
            {
                forwardT(tau, times);
                forwardP(xi, vPolyIdx, vPolytopes, points);
                minco.setParameters(points, times);
                minco.getTrajectory(traj);
            }
            else if(ret == lbfgs::LBFGSERR_MAXIMUMLINESEARCH){
                forwardT(tau, times);
                forwardP(xi, vPolyIdx, vPolytopes, points);
                minco.setParameters(points, times);
                minco.getTrajectory(traj);
                std::cout<<"exceed max iter!!!!!!!!!!!!"<<std::endl;
            }
            else
            {
                traj.clear();
                minCostFunctional = INFINITY;
                std::cout << "Optimization Failed: "
                          << lbfgs::lbfgs_strerror(ret)
                          << std::endl;
            }

            return minCostFunctional;
        }
    };

}

#endif
